#include "System.hpp"

volatile unsigned long int System::tick;
unsigned int System::clock_bus_frequencies[5] = {0};
System::InterruptResource System::interrupt_resources[128] = { 
    {.callback = nullptr,.user_data = nullptr,},
};

NVIC_Register_t *NVIC_register = (NVIC_Register_t*)0xe000e100;
SysTick_Register_t *SysTick_register = (SysTick_Register_t*)0xe000e010;

PWR_Register_t *PWR_register = (PWR_Register_t*)0x40007000;
RCC_Register_t *RCC_register = (RCC_Register_t*)0x40023800;
FLASH_Register_t *FLASH_register = (FLASH_Register_t*)0x40023c00;

void System::initClock(void){
    volatile uint32_t startup_counter = 0,hse_status = 0;
    RCC_register->CR |= ((uint32_t)0x00010000);
    do {
        hse_status = RCC_register->CR & 0x00020000;
        startup_counter++;
    }while ( (hse_status == 0) && (startup_counter < 0x5000) );

    if ( (RCC_register->CR & 0x00020000) != 0 ){
        hse_status = (uint32_t)0x01;
    }else{
        hse_status = (uint32_t)0x00;
    }
    if ( hse_status == (uint32_t)0x01 ){
        RCC_register->APB1ENR |= 0x10000000;
        PWR_register->CR |= 0x0000c000;
        
        RCC_register->CFGR |= 0x00000000;
        RCC_register->CFGR |= 0x00008000;
        RCC_register->CFGR |= 0x00001400;
        RCC_register->PLLCFGR = 8 | (336 << 6) | (((2 >> 1) - 1) << 16) | (0x00400000) | (7 << 24);
        RCC_register->CR |= 0x01000000;
        while ( (RCC_register->CR & 0x02000000) == 0 );
        FLASH_register->ACR = (0x00000100) | (0x00000200) | (0x00000400) | (0x00000005);
        RCC_register->CFGR &= (uint32_t)((uint32_t)~(0x00000003));
        RCC_register->CFGR |= (uint32_t)0x00000002;
        while ( (RCC_register->CFGR & (uint32_t)0x0000000c) != 0x00000008 );
        System::clock_bus_frequencies[(int)ClockBus::AHB1] = 168 * 1000 * 1000;
        System::clock_bus_frequencies[(int)ClockBus::AHB2] = 168 * 1000 * 1000;
        System::clock_bus_frequencies[(int)ClockBus::AHB3] = 168 * 1000 * 1000;
        System::clock_bus_frequencies[(int)ClockBus::APB1] = 168 * 1000 * 1000 / 4;
        System::clock_bus_frequencies[(int)ClockBus::APB2] = 168 * 1000 * 1000 / 2;

    }else{
        /* HSE startup failed. */
    }
}

extern "C"
void System_init(void){
    RCC_register->CR |= (uint32_t)0x00000001;
    RCC_register->CFGR = 0x00000000;
    RCC_register->CR &= (uint32_t)0xfef6ffff;
    RCC_register->PLLCFGR = 0x24003010;
    RCC_register->CR &= (uint32_t)0xfffbffff;
    RCC_register->CIR = 0x00000000;
    System::initClock();
    /* Init SysTick timer. */
    SysTick_register->VAL = 0;
    SysTick_register->LOAD = 21 * 1000;
    SysTick_register->CTRL = (uint32_t)0b11;
}

bool System::setClockChannelStatus(ClockChannel channel,bool status){
    ClockBus clock_bus = (ClockBus)((int)channel / 32);
    int clock_bit = (int)channel % 32;
    if ( status ){
        switch ( clock_bus ){
            case ClockBus::AHB1: RCC_register->AHB1ENR |= ((uint32_t)0x01 << clock_bit); break;
            case ClockBus::AHB2: RCC_register->AHB2ENR |= ((uint32_t)0x01 << clock_bit); break;
            case ClockBus::AHB3: RCC_register->AHB3ENR |= ((uint32_t)0x01 << clock_bit); break;
            case ClockBus::APB1: RCC_register->APB1ENR |= ((uint32_t)0x01 << clock_bit); break;
            case ClockBus::APB2: RCC_register->APB2ENR |= ((uint32_t)0x01 << clock_bit); break;
            default: return false;
        }
    }else{
        switch ( clock_bus ){
            case ClockBus::AHB1: RCC_register->AHB1ENR &= ~((uint32_t)0x01 << clock_bit); break;
            case ClockBus::AHB2: RCC_register->AHB2ENR &= ~((uint32_t)0x01 << clock_bit); break;
            case ClockBus::AHB3: RCC_register->AHB3ENR &= ~((uint32_t)0x01 << clock_bit); break;
            case ClockBus::APB1: RCC_register->APB1ENR &= ~((uint32_t)0x01 << clock_bit); break;
            case ClockBus::APB2: RCC_register->APB2ENR &= ~((uint32_t)0x01 << clock_bit); break;
            default: return false;
        }
    }
    return true;
}

bool System::getClockChannelStatus(ClockChannel channel){
    ClockBus clock_bus = (ClockBus)((int)channel / 32);
    int clock_bit = (int)channel % 32;
    uint32_t temp = 0x00000000;
    switch ( clock_bus ){
        case ClockBus::AHB1: temp = RCC_register->AHB1ENR & (0x01 << clock_bit); break;
        case ClockBus::AHB2: temp = RCC_register->AHB2ENR & (0x01 << clock_bit); break;
        case ClockBus::AHB3: temp = RCC_register->AHB3ENR & (0x01 << clock_bit); break;
        case ClockBus::APB1: temp = RCC_register->APB1ENR & (0x01 << clock_bit); break;
        case ClockBus::APB2: temp = RCC_register->APB2ENR & (0x01 << clock_bit); break;
    }
    if ( temp != 0 ){
        return true;
    }else{
        return false;
    }
}

unsigned int System::getClockChannelFrequency(ClockChannel channel){
    int clock_bus = ((int)channel / 32);
    if ( clock_bus >= 5 ) return 0;
    else return System::clock_bus_frequencies[clock_bus];
}

bool System::setInterruptChannelStatus(InterruptChannel channel,bool status){
    return setInterruptChannelStatus((int)channel,status);
}

bool System::setInterruptChannelStatus(int channel,bool status){
    if ( status ) NVIC_register->ISER[(((uint32_t)channel) >> 5)] = (uint32_t)(1UL << (((uint32_t)channel) & 0x1fUL));
    else NVIC_register->ICER[(((uint32_t)channel) >> 5)] = (uint32_t)(1UL << (((uint32_t)channel) & 0x1fUL));
    return true;
}

bool System::getInterruptChannelStatus(InterruptChannel channel){
    return getInterruptChannelStatus((int)channel);
}

bool System::getInterruptChannelStatus(int channel){
    return true;
}

bool System::addInterruptCallback(InterruptChannel channel,InterruptCallback callback,void *user_data){
    System::interrupt_resources[(int)channel].callback = callback;
    System::interrupt_resources[(int)channel].user_data = user_data;
    return true;
}

void System::delayMs(unsigned int nms){
    unsigned long int start_tick = System::tick;
    while ( System::tick < (start_tick + nms) );
    return;
}

/* Cortex-M4 Processor Exceptions. */

extern "C" 
void SysTick_Handler(void){
    System::tick++;
}

/* STM32 specific interrupts. */

extern "C"{

void WWDG_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::WWDG].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::WWDG].callback(System::interrupt_resources[(int)System::InterruptChannel::WWDG].user_data);
}

void PVD_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::PVD].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::PVD].callback(System::interrupt_resources[(int)System::InterruptChannel::PVD].user_data);
}

void TAMP_STAMP_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::TAMP_STAMP].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::TAMP_STAMP].callback(System::interrupt_resources[(int)System::InterruptChannel::TAMP_STAMP].user_data);
}

void RTC_WKUP_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::RTC_WKUP].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::RTC_WKUP].callback(System::interrupt_resources[(int)System::InterruptChannel::RTC_WKUP].user_data);
}

void FLASH_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::FLASH].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::FLASH].callback(System::interrupt_resources[(int)System::InterruptChannel::FLASH].user_data);
}

void RCC_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::RCC].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::RCC].callback(System::interrupt_resources[(int)System::InterruptChannel::RCC].user_data);
}

void EXTI0_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::EXTI0].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::EXTI0].callback(System::interrupt_resources[(int)System::InterruptChannel::EXTI0].user_data);
}

void EXTI1_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::EXTI1].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::EXTI1].callback(System::interrupt_resources[(int)System::InterruptChannel::EXTI1].user_data);
}

void EXTI2_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::EXTI2].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::EXTI2].callback(System::interrupt_resources[(int)System::InterruptChannel::EXTI2].user_data);
}

void EXTI3_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::EXTI3].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::EXTI3].callback(System::interrupt_resources[(int)System::InterruptChannel::EXTI3].user_data);
}

void EXTI4_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::EXTI4].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::EXTI4].callback(System::interrupt_resources[(int)System::InterruptChannel::EXTI4].user_data);
}

void DMA1_Stream0_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream0].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream0].callback(System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream0].user_data);
}

void DMA1_Stream1_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream1].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream1].callback(System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream1].user_data);
}

void DMA1_Stream2_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream2].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream2].callback(System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream2].user_data);
}

void DMA1_Stream3_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream3].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream3].callback(System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream3].user_data);
}

void DMA1_Stream4_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream4].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream4].callback(System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream4].user_data);
}

void DMA1_Stream5_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream5].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream5].callback(System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream5].user_data);
}

void DMA1_Stream6_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream6].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream6].callback(System::interrupt_resources[(int)System::InterruptChannel::DMA1_Stream6].user_data);
}

void ADC_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::ADC].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::ADC].callback(System::interrupt_resources[(int)System::InterruptChannel::ADC].user_data);
}

void EXTI9_5_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::EXTI9_5].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::EXTI9_5].callback(System::interrupt_resources[(int)System::InterruptChannel::EXTI9_5].user_data);
}

void TIM1_BRK_TIM9_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::TIM1_BRK_TIM9].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::TIM1_BRK_TIM9].callback(System::interrupt_resources[(int)System::InterruptChannel::TIM1_BRK_TIM9].user_data);
}

void TIM1_UP_TIM10_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::TIM1_UP_TIM10].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::TIM1_UP_TIM10].callback(System::interrupt_resources[(int)System::InterruptChannel::TIM1_UP_TIM10].user_data);
}

void TIM1_TRG_COM_TIM11_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::TIM1_TRG_COM_TIM11].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::TIM1_TRG_COM_TIM11].callback(System::interrupt_resources[(int)System::InterruptChannel::TIM1_TRG_COM_TIM11].user_data);
}

void TIM1_CC_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::TIM1_CC].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::TIM1_CC].callback(System::interrupt_resources[(int)System::InterruptChannel::TIM1_CC].user_data);
}

void TIM2_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::TIM2].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::TIM2].callback(System::interrupt_resources[(int)System::InterruptChannel::TIM2].user_data);
}

void TIM3_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::TIM3].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::TIM3].callback(System::interrupt_resources[(int)System::InterruptChannel::TIM3].user_data);
}

void TIM4_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::TIM4].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::TIM4].callback(System::interrupt_resources[(int)System::InterruptChannel::TIM4].user_data);
}

void I2C1_EV_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::I2C1_EV].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::I2C1_EV].callback(System::interrupt_resources[(int)System::InterruptChannel::I2C1_EV].user_data);
}

void I2C1_ER_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::I2C1_ER].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::I2C1_ER].callback(System::interrupt_resources[(int)System::InterruptChannel::I2C1_ER].user_data);
}

void I2C2_EV_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::I2C2_EV].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::I2C2_EV].callback(System::interrupt_resources[(int)System::InterruptChannel::I2C2_EV].user_data);
}

void I2C2_ER_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::I2C2_ER].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::I2C2_ER].callback(System::interrupt_resources[(int)System::InterruptChannel::I2C2_ER].user_data);
}

void SPI1_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::SPI1].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::SPI1].callback(System::interrupt_resources[(int)System::InterruptChannel::SPI1].user_data);
}

void SPI2_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::SPI2].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::SPI2].callback(System::interrupt_resources[(int)System::InterruptChannel::SPI2].user_data);
}

void USART1_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::USART1].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::USART1].callback(System::interrupt_resources[(int)System::InterruptChannel::USART1].user_data);
}

void USART2_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::USART2].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::USART2].callback(System::interrupt_resources[(int)System::InterruptChannel::USART2].user_data);
}

void EXTI15_10_IRQHandler(void){
    if ( System::interrupt_resources[(int)System::InterruptChannel::EXTI15_10].callback != nullptr ) System::interrupt_resources[(int)System::InterruptChannel::EXTI15_10].callback(System::interrupt_resources[(int)System::InterruptChannel::EXTI15_10].user_data);
}

}
